import numpy as np
from robot_arm import RobotArm
from optimizer import PathOptimizer

def solve_problem2():
    # 创建机器人臂实例
    robot = RobotArm()
    optimizer = PathOptimizer(robot)
    
    # 目标点位置
    target_position = np.array([1500, 1200, 200])
    
    # 设置误差约束 (±200mm)
    error_constraint = 200
    
    # 优化路径，同时考虑误差和能耗
    optimal_angles, final_value = optimizer.optimize_path(
        target_position,
        error_weight=0.5,  # 平衡误差和能耗的权重
        energy_weight=0.5,
        error_constraint=error_constraint
    )
    
    # 计算最终位置和误差
    final_position = robot.calculate_end_effector_position(optimal_angles)
    final_error = robot.calculate_end_effector_error(optimal_angles, target_position)
    final_energy = optimizer.calculate_energy(optimal_angles)
    
    # 打印结果
    print("最优关节角度 (弧度):", optimal_angles)
    print("最优关节角度 (度):", np.rad2deg(optimal_angles))
    print("最终位置:", final_position)
    print("末端误差 (mm):", final_error)
    print("总能耗:", final_energy)
    print("综合目标函数值:", final_value)

if __name__ == "__main__":
    solve_problem2() 